function [GPQGPT] = kfprocnoisepsdmat(CBN, INSSensRWC, INSSensStateProcNoisePSDFull, aidStateProcNoisePSD, GPAidBlk, INSSysStateMask, INSSensStateMask) %#codegen
%KFPROCNOISEPSDMAT 计算过程噪声功率谱密度矩阵
%
% Input Arguments:
% CBN: 3*3矩阵，姿态矩阵
% INSSensRWC: 6*1向量，分别为3个加速度计的比速度随机游走系数和3个陀螺的角度随机游走系数，用于速度误差变化率和姿态误差变化率过程噪声方差计算，单位为国际单位制单位
% INSSensStateProcNoisePSDFull: 36*1向量，分别为对应的完整器件状态的过程噪声的功率谱密度，单位为国际单位制单位
% 未被删减的器件状态的过程噪声功率谱密度用于该状态过程噪声方差的计算，被删减的器件状态的功率谱密度在本函数中没有用到，传入以利于统一格式
% 对于挠性惯组g敏感项，如果某项参数的对称参数包含在INS器件状态中，则sensStateProcNoisePSDFull中与该项参数对应的元素应为0
% aidStateProcNoisePSD: 列向量，长度为辅助导航状态过程噪声个数，辅助导航状态的过程噪声功率谱密度
% GPAidBlk: 二维矩阵，行数为辅助导航状态个数，列数为辅助导航过程噪声个数，过程噪声动态耦合矩阵的辅助导航块
% INSSysStateMask: 1*n向量，n不小于9，true表示包含该系统状态，false表示不包含该系统状态
% INSSensStateMask: 1*36向量，true表示包含该器件状态，false表示不包含该器件状态，器件状态依次包含加速度计的3个零偏误差、3个标度因数误差、6个失准角误差、3个二次项系数误差和陀螺的3个零偏误差、3个标度因数误差、6个失准角误差、9个g敏感项误差
%
% Output Arguments:
% GPQGPT: m*m矩阵，m为INSSysStateMask和INSSensStateMask中为true的元素个数之和加上辅助导航状态个数，GPQGPT等价于GP*Q*GPT,其中GP为过程噪声动态耦合矩阵，Q为过程噪声的功率谱密度矩阵，GPT为GP的转置
%
% Assumptions and Limitations:
% 1. 本函数假设完整的系统状态中速度误差为第4-6个元素，姿态误差为第7-9个元素
% 2. 理论上，传入本函数的导航参数应为理想值

coder.extrinsic('warning', 'int2str');

fullINSSenStateNum = 36;
aidStateNum = size(GPAidBlk, 1);
if ((length(INSSensStateMask)~=fullINSSenStateNum) || (length(INSSensStateProcNoisePSDFull)~=fullINSSenStateNum))
    warning('kfprocnoisevarinsblk:INSSensState', ['INS sensor state mask and/or process noise PSD vector length invalid, must be ' int2str(fullINSSenStateNum) '.']);
end

% 过程噪声动态耦合矩阵
% GP = [ 0   0   0   0     位置误差
%      CBN   0   0   0     速度误差
%        0 CBN   0   0     姿态误差
%        0   0   0   0     其它惯性系统状态
%        0   0   I   0     惯性器件状态
%        0   0   0  GPAid] 辅助导航状态

% 过程噪声协方差矩阵 GPQGPT = GP * diag(Q) * GP';
% Q = [Q11  0   0   0      速度误差噪声（加速度计的比速度随机游走）
%      0   Q22  0   0      姿态误差噪声（陀螺的角度随机游走）
%      0    0   Q33 0      器件状态的过程噪声
%      0    0   0   Q44]   辅助导航过程噪声

% GPQGPT = [0       0               0               0	0	0
%           0       CBN*Q11*CBN'    0               0	0   0
%           0       0               CBN*Q22*CBN'    0   0   0
%           0       0               0               0   0	0
%           0       0               0               0   Q33 0
%           0       0               0               0   0   GPAid*Q44*GPAid']

INSSensRWCSquare = INSSensRWC.^2; % TODO: 可以直接传入平方
INSSysStateNum = nnz(INSSysStateMask);
INSSensStateNum = nnz(INSSensStateMask);
GPQGPT_v = CBN*diag(INSSensRWCSquare(1:3))*CBN';
GPQGPT_att = CBN*diag(INSSensRWCSquare(4:6))*CBN';

GPQGPT = zeros(INSSysStateNum+INSSensStateNum+aidStateNum);
GPQGPT_idx0 = nnz(INSSysStateMask(1:3)); % 位置误差
% 速度误差
subStateMask = INSSysStateMask(4:6);
subStateNum = nnz(subStateMask);
GPQGPT_idx = GPQGPT_idx0 + subStateNum;
GPQGPT(GPQGPT_idx0+1:GPQGPT_idx, GPQGPT_idx0+1:GPQGPT_idx) = GPQGPT_v(subStateMask, subStateMask);
GPQGPT_idx0 = GPQGPT_idx;
% 姿态误差
subStateMask = INSSysStateMask(7:9);
subStateNum = nnz(subStateMask);
GPQGPT_idx = GPQGPT_idx0 + subStateNum;
GPQGPT(GPQGPT_idx0+1:GPQGPT_idx, GPQGPT_idx0+1:GPQGPT_idx) = GPQGPT_att(subStateMask, subStateMask);
% 惯性器件状态
INSSensStateProcNoisePSD = INSSensStateProcNoisePSDFull(INSSensStateMask);
for i = 1:INSSensStateNum
    GPQGPT(i+INSSysStateNum, i+INSSysStateNum) = INSSensStateProcNoisePSD(i);
end
% 辅助导航状态
GPQGPT((INSSysStateNum+INSSensStateNum+1):(INSSysStateNum+INSSensStateNum+aidStateNum), (INSSysStateNum+INSSensStateNum+1):(INSSysStateNum+INSSensStateNum+aidStateNum)) = GPAidBlk*diag(aidStateProcNoisePSD)*GPAidBlk';
end